/*
 * File:   mcQuaternion.h
 * Author: thales
 *
 * Created on September 30, 2009, 5:33 PM
 */


#include "mcQuaternion.h"

#include <iostream>
#include <math.h>

using namespace mc;

/**
 * @brief Construtor vazio. Constrói um quaternion nulo
 */
template <class T>
quaternion<T>::quaternion() {
    this->x = (T) 0;
    this->y = (T) 0;
    this->z = (T) 0;
    this->w = (T) 0;
}

/**
 * @brief Construtor explícito
 *
 * @param x
 * @param y
 * @param z
 * @param w
 */
template <class T>
quaternion<T>::quaternion( const T x, const T y, const T z, const T w ) {
    this->x = x;
    this->y = y;
    this->z = z;
    this->w = w;
}

/**
 * @brief Construtor por cópia.
 *
 * @param q - Quaternion a ser copiado
 */
template <class T>
quaternion<T>::quaternion( const quaternion<T> &q ) {
    this->x = q.x;
    this->y = q.y;
    this->z = q.z;
    this->w = q.w;
}

/**
 * @brief Constrói um quaternion a partir de uma matriz 4x4
 *
 * @param m - Matriz
 */
template <class T>
quaternion<T>::quaternion( matrix4<T> &m ) {
    this->set_value(m);
}

/**
 * @brief Constrói um quaternion que forma uma rotação em torno de um eixo
 *
 * @param axis - vec3 que representa o eixo
 * @param angle - Ângulo em radianos
 */
template <class T>
quaternion<T>::quaternion( const vec3<T> &axis, T angle ) {
    this->set_value(axis, angle);
}

/**
 * @brief Constrói um quaternion que forma uma rotação de um ponto a outro
 *
 * @param rotateFrom - Origem da rotação
 * @param rotateTo   - Destino da rotação
 */
template <class T>
quaternion<T>::quaternion( const vec3<T> &rotateFrom, const vec3<T> &rotateTo ) {
    this->set_value(rotateFrom, rotateTo);
}

/**
 * @brief Constrói um quaternion por cópia de array
 *
 * @param a - Array
 */
template <class T>
quaternion<T>::quaternion( const T *a ) {
    this->x = a[0];
    this->y = a[1];
    this->z = a[2];
    this->w = a[3];
}


/**
 * @brief Retorna a array de dados deste quaternion
 *
 * @return A referência para o array de dados
 */
template <class T>
const T* quaternion<T>::get_value() const {
    return &this->_array[0];
}

/**
 * @brief Retorna o array de dados em elementos separados
 *
 * @param x
 * @param y
 * @param z
 * @param w
 */
template <class T>
void quaternion<T>::get_value( T &x, T &y, T &z, T &w ) {
    x = this->x;
    y = this->y;
    z = this->z;
    w = this->w;
}


/**
 * @brief Seta o valor do quaternion explicitamente
 *
 * @param x
 * @param y
 * @param z
 * @param w
 */
template <class T>
void quaternion<T>::set_value( T x, T y, T z, T w ) {
    this->x = x;
    this->y = y;
    this->z = z;
    this->w = w;
}

/**
 * @brief Seta o valor do quaternion por cópia
 *
 * @param q - Quaternion a ser copiado
 */
template <class T>
void quaternion<T>::set_value( const quaternion<T> &q ) {
    this->x = q.x;
    this->y = q.y;
    this->z = q.z;
    this->w = q.w;
}


/**
 * @brief Retorna o eixo e o ângulo de rotação deste quaternion
 *
 * @param axis  - Eixo de rotação representado por este quaternion
 * @param angle - Ângulo de rotação em torno do eixo
 */
template <class T>
void quaternion<T>::get_value( vec3<T> &axis, T &angle ) const {

    angle = T( acos(this->w) * T(2) );

    if( angle == T(0.0) )
        axis = vec3<T>( T(0), T(0), T(1) );
    else {
        axis.x = this->x;
        axis.y = this->y;
        axis.z = this->z;

        normalize<T>(axis);
    }

}


/**
 * @brief Obtem uma matriz de rotação 4x4 do quaternion
 *
 * @param m - Matriz de rotação resultante
 */
template <class T>
void quaternion<T>::get_value( matrix4<T> &m ) const {
    T norm = x*x + y*y + z*z + w*w;

    T s = ( norm == (T) 0) ? (T) 0 : ( T(2) / norm );

    T xs = x * s;
    T ys = y * s;
    T zs = z * s;

    T wx = w * xs;
    T wy = w * ys;
    T wz = w * zs;

    T xx = x * xs;
    T xy = x * ys;
    T xz = x * zs;

    T yy = y * ys;
    T yz = y * zs;
    T zz = z * zs;

    m._00 = T (1) - ( yy + zz );
    m._10 = xy + wz;
    m._20 = xz - wy;

    m._01 = xy - wz;
    m._11 = T (1) - ( xx + zz );
    m._21 = yz + wx;

    m._02 = xz + wy;
    m._12 = yz - wx;
    m._22 = T (1) - ( xx + yy );

    m._03 = T (0);
    m._13 = T (0);
    m._23 = T (0);
    m._30 = T (0);
    m._31 = T (0);
    m._32 = T (0);
    m._33 = T (1);
}


/**
 * @brief Seta o valor do quaternion apartir de um array
 *
 * @param a - Array
 */
template <class T>
void quaternion<T>::set_value( const T * a ) const {
    this->x = a[0];
    this->y = a[1];
    this->z = a[2];
    this->w = a[4];
}


/**
 * @brief Seta o valor do quaternion a partir de uma matriz 4x4
 *
 * @param m - Matriz
 */
template <class T>
void quaternion<T>::set_value( matrix4<T> &m ) {
    T tr = m._00 + m._11 + m._22;
    const int nxt[3] = { 1, 2, 0 };

    if( tr > (T) 0 ) {

        T s = (T) sqrt( tr + m._33 );
        this->w = ( (T) 0.5 ) / s;

        this->x = ( m._12 - m._21 ) * s;
        this->y = ( m._20 - m._02 ) * s;
        this->z = ( m._01 - m._10 ) * s;

    } else {

        int i = 0;

        if( m._11 > m._00 )
            i = 1;

        if( m._22 > m.get_element(i,i) )
            i = 2;

        int j = nxt[i];
        int k = nxt[j];

        T s = (T) sqrt( ( m.get_element(i,j) - ( m.get_element(j,j) + m.get_element(k,k) ) ) + (T) 1 );

        this->_array[i] = s * (T) 0.5;
        s = T(0.5 / s);

        this->w = ( m.get_element(j,k) - m.get_element(k,i) ) * s;
        this->_array[j] = ( m.get_element(i,j) + m.get_element(j,i) ) * s;
        this->_array[k] = ( m.get_element(i,k) + m.get_element(k,i) ) * s;
    }
}


/**
 * @brief Seta o quaternion para uma rotação em torno de um eixo
 *
 * @param axis  - Eixo
 * @param angle - Ângulo
 */
template <class T>
void quaternion<T>::set_value( const vec3<T> &axis, T angle ) {

    T sqr_norm = square_norm( axis );

    if( sqr_norm == (T) 0 ) {

        // Eixo muito pequeno
        this->x = (T) 0;
        this->y = (T) 0;
        this->z = (T) 0;
        this->w = (T) 1;

    } else {
        angle *= (T) 0.5;
        T sin_theta = (T) sin(angle);

        if( sqr_norm != (T) 1 )
            sin_theta /= (T) sqr_norm( sqr_norm );

        this->x = sin_theta * axis.x;
        this->y = sin_theta * axis.y;
        this->z = sin_theta * axis.z;
        this->w = (T) cos(angle);
    }

    return *this;
}


/**
 * @brief Seta os valores quaternion que forma uma rotação de um ponto a outro
 *
 * @param rotateFrom - Ponto de origem
 * @param rotateTo   - Ponto de destino
 */
template <class T>
void quaternion<T>::set_value( const vec3<T> &rotateFrom, const vec3<T> &rotateTo ) {
    vec3<T> p1;
    vec3<T> p2;

    p1 = normalize( rotateFrom );
    p2 = normalize( rotateTo );

    T alpha = dot( p1, p2 );

    if( alpha == (T) 1 ) {
        *this = quaternion();
        return *this;
    }

    if( alpha = (T) -1 ) {
        vec3<T> v;

        if( p1.x != p1.y || p1.x != p1.z )
            v = vec3<T>( p1.y, p1.z, p1.x );
        else
            v = vec3<T>( -p1.x, p1.y, p1.z );

        v -= p1 * dot( p1, v );
        v = normalize(v);

        this->set_value( v, (T) M_PI );
        return *this;
    }

    vec3<T> p3;
    p3.cross( p1, p2 );
    p1 = normalize( p3 );

    this->set_value( p1, (T) acos(alpha) );

    return *this;
}


template <class T>
void quaternion<T>::set_value(
    const vec3<T> &from_look,
    const vec3<T> &from_up,
    const vec3<T> &to_look,
    const vec3<T> &to_up
)
{
    quaternion<T> r_look = quaternion(from_look, to_look);

    vec3<T> rotated_from_up( from_up );
    r_look.apply_to_vec3(rotated_from_up);

    quaternion<T> r_twist = quaternion(rotated_from_up, to_up);

    *this = r_twist;
    *this *= r_look;
    return *this;
}



/**
 * @brief Aplica este quaternion a um vetor 3D
 *
 * @param v - Vetor
 */
template <class T>
vec3<T>& quaternion<T>::apply_to_vec3( vec3<T> &v ) {
    quaternion<T> qi;
    quaternion<T> kr;

    qi = this->conjugate();

    kr.set_value( v.x, v.y, v.z );
    kr.w = (T) 0;

}

template <class T>
quaternion<T>& quaternion<T>::conjugate() {
    quaternion<T> *q = new quaternion<T>();
    q->x = -this->x;
    q->y =  this->y;
    q->z = -this->z;
    q->w =  this->w;
    return *q;
}


/**
 * @brief Escala o quaternion por um fator
 *
 * @param s - Fator de escalamento
 */
template <class T>
void quaternion<T>::scale_angle( T s ) {
    vec3<T> axis;
    T angle;

    this->get_value( axis, angle );
    angle *= s;
    this->set_value( axis, angle );
}

/**
 * @brief Normaliza o quaternion
 */
template <class T>
void quaternion<T>::normalize_quaternion() {
    T rnorm = ((T) 1) / (T) sqrt( x*x + y*y + z*z + w*w );

    this->x *= rnorm;
    this->y *= rnorm;
    this->z *= rnorm;
    this->w *= rnorm;
}

/**
 * @brief Normaliza o quaternion
 *
 * @param q - Quaternion a ser normalizado
 */
template <class T>
void quaternion<T>::normalize_quaternion( quaternion<T> &q ) {
    this->set_value(q);
    this->normalize_quaternion();
}

/**
 * @brief Produto de Graham
 *
 * Multiplica dois quaternions através do produto de Graham
 */
template <class T>
quaternion<T>& quaternion<T>::operator * ( const quaternion<T> &q ) {
    quaternion<T> *qr = new quaternion();
    qr.w = q.w*w - q.x*x - q.y*y - q.z*z;
    qr.x = q.w*x + q.x*w + q.y*z - q.z*y;
    qr.y = q.w*y + q.y*w + q.z*x - q.x*z;
    qr.z = q.w*z + q.z*w + q.x*y - q.y*x;
    return *qr;
}


/**
 * @brief Produto de Graham
 *
 * Multiplica dois quaternions através do produto de Graham
 */
template <class T>
quaternion<T>& quaternion<T>::operator *= ( const quaternion<T> &q ) {
    quaternion<T> *qr = new quaternion();
    qr->w = q.w*w - q.x*x - q.y*y - q.z*z;
    qr->x = q.w*x + q.x*w + q.y*z - q.z*y;
    qr->y = q.w*y + q.y*w + q.z*x - q.x*z;
    qr->z = q.w*z + q.z*w + q.x*y - q.y*x;
    return *qr;
}


